/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.Timer;

/**
 *
 * @author Administrator
 */
public class ArmCrioPID {
    private Jaguar m_shoulder;
    private Jaguar m_elbow;
    private PIDController m_shoulderPID;
    private PIDController m_elbowPID;
    private AnalogChannel m_shoulderPOT;
    private AnalogChannel m_elbowPOT;
    private DriverStationEnhancedIO m_enhancedIO;
    private int m_shoulderWaldoChannel;
    private int m_elbowWaldoChannel;
    private double m_shoulderValue = 0;
    private double m_elbowValue = 0;
    
    // This is correct. The down value is greater than the up value on the Waldo.
    private static final double kDownShoulderVoltage = 2.7;
    private static final double kUpShoulderVoltage = 1.3;
    private static final double kRetractedElbowVoltage = 2.8;
    private static final double kExtendedElbowVoltage = 0.9;

    public static final double kDownShoulderPosition = 0.09; // TODO
    public static final double kUpShoulderPosition = 0.55; // TODO
    public static final double kRetractedElbowPosition = 0.084; // TODO
    public static final double kExtendedElbowPosition = 0.665; // TODO

    public static final double kElbowMinToBeCollecting = .4;
    public static final double kShoulderMinWhenElbowInCollecting = 0.151;

    private DriverStation m_ds = DriverStation.getInstance();

    public ArmCrioPID(int analogSlot, int shoulderPOT, int elbowPOT, int shoulderSlot, int shoulderChannel, int elbowSlot, int elbowChannel, int shoulderWaldoChannel, int elbowWaldoChannel) {
        m_shoulder = new Jaguar(shoulderSlot, shoulderChannel);
        m_elbow = new Jaguar(elbowSlot, elbowChannel);
        m_shoulderPOT = new AnalogChannel(analogSlot, shoulderPOT);
        m_elbowPOT = new AnalogChannel(analogSlot, elbowPOT);
        m_shoulderPID = new PIDController(1000, 0, 100, m_shoulderPOT, m_elbow);
        m_elbowPID = new PIDController(700, 0, 50, m_elbowPOT, m_elbow);
        m_shoulderPID.setInputRange(0.0, 1.0); // TODO
        m_elbowPID.setInputRange(0.0, 1.0); // TODO
        m_shoulderPID.setOutputRange(0.0, 0.60); // TODO
        m_elbowPID.setOutputRange(0.0, 0.60); // TODO
        m_shoulderPID.enable();
        m_elbowPID.enable();
        m_enhancedIO = DriverStation.getInstance().getEnhancedIO();
        m_shoulderWaldoChannel = shoulderWaldoChannel;
        m_elbowWaldoChannel = elbowWaldoChannel;
    }

    public void getValues() {
        try {
            m_shoulderValue = m_enhancedIO.getAnalogIn(m_shoulderWaldoChannel);
            m_shoulderValue -= kDownShoulderVoltage;
            m_shoulderValue /= (kUpShoulderVoltage - kDownShoulderVoltage);
            if (m_shoulderValue < 0) {
                m_shoulderValue = 0;
            }
            if (m_shoulderValue > 1) {
                m_shoulderValue = 1;
            }

            m_elbowValue = m_enhancedIO.getAnalogIn(m_elbowWaldoChannel);
            m_elbowValue -= kRetractedElbowVoltage;
            m_elbowValue /= (kExtendedElbowVoltage - kRetractedElbowVoltage);
            if (m_elbowValue < 0) {
                m_elbowValue = 0;
            }
            if (m_elbowValue > 1) {
                m_elbowValue = 1;
            }
        } catch (DriverStationEnhancedIO.EnhancedIOException e) {
            DriverStationPrinter.getInstance().println("Waldo Pos Read Error!");
        }
    }

    public void update() {
        getValues();
        double dMotorValueShoulder = m_shoulderValue;
        dMotorValueShoulder *= (kUpShoulderPosition - kDownShoulderPosition);
        dMotorValueShoulder += kDownShoulderPosition;

        double dMotorValueElbow = m_elbowValue;
        dMotorValueElbow *= (kExtendedElbowPosition - kRetractedElbowPosition);
        dMotorValueElbow += kRetractedElbowPosition;

        // Make sure that when the elbow is extended to collect that the shoulder stays
        // up enough to prevent the forarm from hitting the front of the robot.
        if ((dMotorValueElbow > kElbowMinToBeCollecting) && (dMotorValueShoulder < kShoulderMinWhenElbowInCollecting)){
            dMotorValueShoulder = kShoulderMinWhenElbowInCollecting;
        }


        m_shoulderPID.setSetpoint(dMotorValueShoulder);
        m_elbowPID.setSetpoint(dMotorValueElbow);

    }

    // Must call getValues or update first on the next two methods.
    public double getSholderValue() {
        return m_shoulderValue;
    }

    public double getElbowValue() {
        return m_elbowValue;
    }
    
    /*
     *
     * @param shoulderPosition The position value for the shoulder in motor counts.
     * @param elbowPosition The position value for the elbow in motor counts.
     */
    public void setAutonomousArmPositions(double shoulderPosition, double elbowPosition) {
        if (m_ds.isAutonomous()) {
            // Make sure the shoulder position is in range.
            if (shoulderPosition > kUpShoulderPosition) {
                shoulderPosition = kUpShoulderPosition;
            } else if (shoulderPosition < kDownShoulderPosition) {
                shoulderPosition = kDownShoulderPosition;
            }
            // Make sure the elbow position is in range.
            if (elbowPosition > kExtendedElbowPosition) {
                elbowPosition = kExtendedElbowPosition;
            } else if (elbowPosition < kRetractedElbowPosition) {
                elbowPosition = kRetractedElbowPosition;
            }


            m_shoulderPID.setSetpoint(shoulderPosition);
            m_elbowPID.setSetpoint(elbowPosition);
        }
    }

    /**
     * Updates the information on the SmartDashboard.
     */
    public void updateSmartDashboard() {      
        SmartDashboard.log(m_shoulderPOT.pidGet(), "Shoulder Position");
        SmartDashboard.log(m_elbowPOT.pidGet(), "Elbow Position");
    }
}
